#include <Arduino.h>
#include <math.h>
#include <U8g2lib.h>
#include <servo1.h>
U8G2_SSD1306_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/U8X8_PIN_NONE, /* clock=*/22, /* data=*/21);

//#define timer 13
#define IO 8   //时钟同步
#define MIC1 4 //声音输入
#define MIC2 5

const int d = 10;  //孔径：10cm
const int c = 340; //声速：20cm

double r1;     //角1(过程角）
double angle1; //所求角(为横向角)
double h;      //长度

//int freq = 50;      // 频率(20ms周期)
int channel1 = 8;   // 通道(高速通道（0 ~ 7）由80MHz时钟驱动，低速通道（8 ~ 15）由 1MHz 时钟驱动。)
// int resolution = 8; // 分辨率

//int channel2 = 9; // 通道(高速通道（0 ~ 7）由80MHz时钟驱动，低速通道（8 ~ 15）由 1MHz 时钟驱动。)

//const int servo1 = 16;
//const int servo2 = 16;

// 声明一个定时器变量
hw_timer_t *timer = NULL;

uint32_t interruptCount; //中断计时，单位为1us
uint32_t t1;
uint32_t t2;

//舵机角度计算///=============库函数里有
// int calculatePWM(double degree)
// {
//   //0-180度
//   //20ms周期，高电平0.5-2.5ms，对应0-180度角度
//   const float deadZone = 6.4; //对应0.5ms（0.5ms/(20ms/256）)
//   const float max = 32;       //对应2.5ms
//   if (degree < 0)
//     degree = 0;
//   if (degree > 180)
//     degree = 180;
//   return (int)(max - ((max - deadZone) / 180) * degree);
// }

void Servopull(int x1, int y1)
{
  ledcWrite(channel, calculatePWM(angle1)); // 输出横向PWM
  //ledcWrite(channel2, calculatePWM(angle2)); // 输出纵向PWM
}
//定时器中断
void IRAM_ATTR onTimer()
{
  interruptCount += 2; //1us
}
//IO清零
void bzero()
{
  interruptCount = 0;
  Serial.println("IRQ");
}
void bmic1()
{
  t1 = interruptCount % 1000000;
  t1 = t1 / 1000000;
  Serial.println(t1);
}
void bmic2()
{
  t2 = interruptCount % 1000000; //us
  t2 = t2 / 1000000;             //s
  Serial.println(t2);
}

void setup()
{ //显示屏初始化
  u8g2.begin();
  u8g2.enableUTF8Print();  
  //引脚配置
  Serial.begin(115200); // 配置串口
  //pinMode(IO, INPUT|PULLDOWN );
  pinMode(IO, INPUT | PULLUP); //上拉输入，当IO输入为高电平时，令t=0
  pinMode(MIC1, INPUT | PULLUP);
  pinMode(MIC2, INPUT | PULLUP);

  //pinMode(MIC1N, OUTPUT);
  //pinMode(MIC2N, OUTPUT);
  attachInterrupt(IO, bzero, RISING); //同步时钟

  // ledcSetup(channel, freq, resolution); // 设置舵机通道===========servo1.cpp里面有
  // ledcAttachPin(servo1, channel);       // 舵机将通道与对应的引脚连接

  //定时器设置相关代码
  timer = timerBegin(0, 16, true);             // 配置定时器 这里使用的是定时器0(一共四个0123) // 这里设置80 就是1Mhz 就能保证定时器0.2us记录一次  // true表面该定时器向上计数
                                               //初始化完毕，将定时器连接到中断：
  timerAttachInterrupt(timer, &onTimer, true); // 配置定时器的中断函数 true表示边沿触发
  //定时：         操作的定时器                  定时时长                数值是否重载
  timerAlarmWrite(timer, 10, true); // 设置定时器的报警值 当计时器计数值达到1000000时触发中断    //  2us记录一次                     //1s=1000000us

  timerAlarmEnable(timer); // 开始启动定时器：使能定时器报警
}
//二值化
void NE55()
{

  if ((analogRead(MIC1) > 1650))
  {
    //MIC1N = 1;
    //digitalWrite(MIC1N,HIGH);
    delay(10);
  }
  else
  {
    //MIC1N = 0;
    //digitalWrite(MIC1N,LOW);
    delay(10);
  }
  if (analogRead(MIC2) > 1650)
  {
    //MIC2N = 1;
    delay(10);
  }
  else
  {
    // MIC2N = 0;
    delay(10);
  }
}
//OLED显示函数
void display()
{
  u8g2.setFont(u8g2_font_wqy12_t_gb2312); //此处建议用gb2312字库，完整支持所有中文编码
  u8g2.setFontDirection(0);
  u8g2.firstPage();
  do
  {
    u8g2.setCursor(0, 20);
    u8g2.print("角度:");
    //Serial.print(angle);
    u8g2.setCursor(0, 40);
    u8g2.print("距离:");
    Serial.print(h);
  } while (u8g2.nextPage());

  delay(1000);
}

void count()
{

  r1 = acos((t2 * c * t2 * c + d * d - t1 * c * t1 * c) / (2 * t2 * c * d));
  //angle = acos(sin(r1)); //角度
  //所求距离
  h = t1 * c * sin(r1);
  //cos(r1) = (t2*t2 + d*d -t1*t1) / (2*b*c);
  //cos(r2) = sin (r1)

  //所求长度
  //h = t1*sin(r1);
}

void loop(void)
{
 u8g2.setFont(u8g2_font_wqy12_t_gb2312); //此处建议用gb2312字库，完整支持所有中文编码
 u8g2.firstPage();
 do
 {
   u8g2.setCursor(0, 20);
   u8g2.print("角度"); 
   u8g2.setCursor(0, 40);
   u8g2.print("距离"); 
 } while (u8g2.nextPage());
 delay(1000);
}
